tts('Program starting ...')
tic

MoveToPosition(0,90,0,50)
Claw(0)
kinect_code_1
p1 = edgeroo(img_1_colour);
tts('large cup one')
[x,y,z] = inv_kin(arm_large(1,1)-0.045,arm_large(1,2),arm_large(1,3)-0.01);
MoveToPosition(-x,y,z,50)
PickDropCup(2)
[x,y,z] = inv_kin(p1(1),1.1*p1(2)+0.04,p1(3)+0.14);
MoveToPosition(0,90,0,50)
MoveToPosition(-x,y,z,50)
PickDropCup(1)
toc

% tts('large cup two')
% [x,y,z] = inv_kin(arm_large(2,1)-0.04,arm_large(2,2),arm_large(2,3)+.02);
% MoveToPosition(-x,y,z,100)
% PickDropCup(2)
% MoveToPosition(-2*x,y,z,100);p1 = edgeroo(img_1_colour)
% PickDropCup(1)
% MoveToPosition(0,90,0,100)

% tts('medium cup one')
% MoveToPosition(0,90,0,50)
% Claw(0)
% [x,y,z] = inv_kin(arm_med(1,1)-0.035,arm_med(1,2),arm_med(1,3)+.04);
% MoveToPosition(-x,y,z,100)
% PickDropCup(2)
% MoveToPosition(-x+90,y,z,100);
% PickDropCup(1)
% MoveToPosition(0,90,0,100)
% 
% MoveToPosition(-90,90,0,50)
tts('Order complete')
% Claw(0)
% 
% tts('medium cup two')
% [x,y,z] = inv_kin(arm_med(2,1)-0.035,arm_med(2,2),arm_med(2,3)+.04);
% MoveToPosition(-x,y,z,100)
% PickDropCup(2)
% MoveToPosition(-2*x,y,z,100);
% PickDropCup(1)
% MoveToPosition(0,90,0,100)
%tts('are you not impressed?')